"""
  此文件从过滤的数据中提取出跟随数据
"""

# !处理路径导入问题（添加绝对路径）！！！
import sys
import os
CODE_INTERNAL_PATH = os.path.abspath(os.path.join(os.path.dirname(__file__), '..')) # 生成Code文件夹内部对应的绝对路径
sys.path.append(CODE_INTERNAL_PATH)

# 导入内部包
from utils.read_data import read_original_data
from utils.write_data import write_data_to_file

# 文件路径常量
FILE_PATH_I80_1_from = "../../Data/Ngsim数据集/I80数据集/2. 过滤数据/trajectories-0400-0415_filter.txt" # 2052 884 18
FILE_PATH_I80_2_from = "../../Data/Ngsim数据集/I80数据集/2. 过滤数据/trajectories-0500-0515_filter.txt" # 1836 526 18
FILE_PATH_I80_3_from = "../../Data/Ngsim数据集/I80数据集/2. 过滤数据/trajectories-0515-0530_filter.txt" # 1790 1153 18
FILE_PATH_101_1_from = "../../Data/Ngsim数据集/101数据集/2. 过滤数据/trajectories-0750am-0805am_filter.txt" # 2169 437 18
FILE_PATH_101_2_from = "../../Data/Ngsim数据集/101数据集/2. 过滤数据/trajectories-0805am-0820am_filter.txt" # 2017 569 18
FILE_PATH_101_3_from = "../../Data/Ngsim数据集/101数据集/2. 过滤数据/trajectories-0820am-0835am_filter.txt" # 1915 421 18

FILE_PATH_I80_1_to = "../../Data/Ngsim数据集/I80数据集/3. 提取数据/1. 跟随数据/trajectories-0400-0415_follow.txt" # 2357 200 14 / 过滤非法数据后 1182 200 14（师兄原始的实验数据量 1244 200 5）
FILE_PATH_I80_2_to = "../../Data/Ngsim数据集/I80数据集/3. 提取数据/1. 跟随数据/trajectories-0500-0515_follow.txt" # 3420 200 14 / 过滤非法数据后 1545 200 14
FILE_PATH_I80_3_to = "../../Data/Ngsim数据集/I80数据集/3. 提取数据/1. 跟随数据/trajectories-0515-0530_follow.txt" # 3913 200 14 / 过滤非法数据后 1490 200 14
FILE_PATH_101_1_to = "../../Data/Ngsim数据集/101数据集/3. 提取数据/1. 跟随数据/trajectories-0750am-0805am_follow.txt" # 2637 200 14 / 过滤非法数据后 1937 200 14
FILE_PATH_101_2_to = "../../Data/Ngsim数据集/101数据集/3. 提取数据/1. 跟随数据/trajectories-0805am-0820am_follow.txt" # 3652 200 14 / 过滤非法数据后 2640 200 14
FILE_PATH_101_3_to = "../../Data/Ngsim数据集/101数据集/3. 提取数据/1. 跟随数据/trajectories-0820am-0835am_follow.txt" # 4166 200 14 / 过滤非法数据后 2865 200 14

# 其他常量
TARGET_FRAMES = 200 # 目标帧数（要收集的数据数量）
STABLE_FRAMES = 250 # 达到稳定跟驰状态的最小帧数
EFFECTIVE_FOLLOW_SPACE = 100 # 有效的跟驰距离（当超过这个距离时，不认为有跟驰效果）

# !判断是否维持这以前的跟驰状态
def is_following(pre_front_carId, cur_front_carId):
  if (pre_front_carId == 0):
    # 说明当前车是第一个编号的车，不可能存在头车
    return False
  
  # 前后ID判定
  if cur_front_carId != pre_front_carId:
    # 非跟驰状态
    return False
  
  # 跟驰状态
  return True

def is_valid_lane(cur_lane):
  return cur_lane >= 1 and cur_lane <= 5

# 收集一组跟驰数据
def collect_follow_data(filter_data, self_car_data, front_carId, self_car_data_start_idx , carId_idx=0, frameId_idx=1):
  follow_data = []

  # 计算收集的起始帧
  self_car_start_frame = self_car_data[self_car_data_start_idx][frameId_idx]

  # 前车数据
  front_car_data = []
  # 前车数据起始索引
  front_car_data_start_idx = 0

  # !找前车数据（基于前车ID）
  front_car_data = find_data_by_carId(filter_data, front_carId, carId_idx)

  # !找前车数据的起始索引（基于自车起始帧）
  front_car_data_start_idx = find_start_idx_by_frame(front_car_data, self_car_start_frame, frameId_idx)

  # !开始数据收集（两个数组合并成一个数组）
  for i in range(TARGET_FRAMES):
    front_car_item = front_car_data[front_car_data_start_idx + i]
    self_car_item = self_car_data[self_car_data_start_idx + i]

    # 安全性校验
    if(front_car_item[frameId_idx] != self_car_item[frameId_idx]):
      print("数据帧不一致", front_car_item[frameId_idx], self_car_item[frameId_idx])

    # !直接合并数据，并且做收集
    follow_data.append(format_follow_data(front_car_item, self_car_item))

  return follow_data

def find_data_by_carId(filter_data, target_carId, carId_idx):
  for i in range(len(filter_data)):
    if filter_data[i][0][carId_idx] == target_carId:
      # 找到前车数据
      return filter_data[i]

def find_start_idx_by_frame(data, target_frame, frameId_idx):
  for j in range(len(data)):
    if data[j][frameId_idx] == target_frame:
      # 找到目标起始帧
      return j

# 对生成的跟随数据做格式化
def format_follow_data(front_car_item, self_car_item):
  combine_data = []

  # 数据帧
  combine_data.append(self_car_item[1])

  # 编号
  combine_data.append(front_car_item[0])
  combine_data.append(self_car_item[0])

  # X坐标（横轴）
  combine_data.append(front_car_item[4])
  combine_data.append(self_car_item[4])

  # Y坐标（纵轴）
  combine_data.append(front_car_item[5])
  combine_data.append(self_car_item[5])

  # 速度
  combine_data.append(front_car_item[11])
  combine_data.append(self_car_item[11])

  # 加速度
  combine_data.append(front_car_item[12])
  combine_data.append(self_car_item[12])

  # 车道编号
  combine_data.append(self_car_item[13])

  # 车头间距
  combine_data.append(abs(front_car_item[5] - self_car_item[5])) # todo 计算的对吗？

  # 车头时距
  # todo 这里的车头时距应该重新基于位置和速度来算
  combine_data.append(self_car_item[17])

  return combine_data

# 提取跟驰数据
def extract_follow_data(filter_datas, lane_idx=13, front_carId_idx=14):
  # 收集到的若干跟驰数据组
  follow_datas = []

  # 当前稳定达到的跟驰帧数
  accumulated_follow_frams = 0

  for i in range(len(filter_datas)):
    # 当前车辆，上一帧的头车Id
    pre_front_carId = 0

    for j in range(len(filter_datas[i])):
      # 读取数据（当前 车道Id、前车Id）
      cur_laneId = filter_datas[i][j][lane_idx]
      cur_front_carId = filter_datas[i][j][front_carId_idx]

      if not is_following(pre_front_carId, cur_front_carId) or not is_valid_lane(cur_laneId):
        # 条件不符，重新统计
        accumulated_follow_frams = 1
      else:
        # 条件符合，统计帧数
        accumulated_follow_frams += 1
        
        if accumulated_follow_frams >= STABLE_FRAMES:
          # !达到稳定跟驰帧数，形成一组跟驰数据
          # 收集跟驰数据
          follow_data = collect_follow_data(filter_datas, filter_datas[i], cur_front_carId, j - STABLE_FRAMES + 1)
          follow_datas.append(follow_data)

          # !尝试继续向后检测（有可能当前前后车相同的一对跟驰数据中，可以提取出两组数据）
          accumulated_follow_frams = 0
      
      pre_front_carId = cur_front_carId

  return follow_datas

# 过滤掉有问题的组（例如，前后车位置重叠等）
def filter_error_group(follow_datas, front_car_pos_idx=5, self_car_pos_idx=6):
  filtered_datas = []

  has_error_data = False

  for i, follow_data in enumerate(follow_datas):
    # 检测这组数据是否有问题
    for j, data in enumerate(follow_data):
      if not is_valid_follow_data(data, front_car_pos_idx, self_car_pos_idx):
        print("出现一条不符合规则的跟车数据", i, j)
        has_error_data = True
        break
    
    if not has_error_data:
      # 收集这组数据
      filtered_datas.append(follow_data)

    has_error_data = False

  return filtered_datas


def is_valid_follow_data(data, front_car_pos_idx=5, self_car_pos_idx=6, front_car_v_idx=7, self_car_v_idx=8):
  cur_space = data[front_car_pos_idx] - data[self_car_pos_idx]
  front_car_v = data[front_car_v_idx]
  self_car_v = data[self_car_v_idx]

  # 限制车头间距（这里的0可以再优化，例如设置成可接受的最小间距）# todo 这个部分的 EFFECTIVE_FOLLOW_SPACE 设置成了定值，但是我感觉可以基于车速来动态生成（可以后续做优化）
  if cur_space <= 0 or cur_space > EFFECTIVE_FOLLOW_SPACE:
    return False
  # 限制车速
  if front_car_v <= 0 or self_car_v <= 0:
    return False

  return True

def handle(fromFileName, toFileName, index):
  # 读取数据
  filter_datas = read_original_data(fromFileName)

  # 提取跟驰数据
  follow_datas = extract_follow_data(filter_datas)

  # 过滤出含非法值的数据组
  filtered_datas = filter_error_group(follow_datas)

  print("第几组数据", index + 1)
  print(len(follow_datas), len(follow_datas[0]), len(follow_datas[0][0]))
  print(len(filtered_datas), len(filtered_datas[0]), len(filtered_datas[0][0]))

  # 写入数据
  # write_data_to_file(filtered_datas, toFileName)

if __name__ == '__main__':
  fileNames = [
    [FILE_PATH_I80_1_from, FILE_PATH_I80_1_to],
    [FILE_PATH_I80_2_from, FILE_PATH_I80_2_to],
    [FILE_PATH_I80_3_from, FILE_PATH_I80_3_to],
    [FILE_PATH_101_1_from, FILE_PATH_101_1_to],
    [FILE_PATH_101_2_from, FILE_PATH_101_2_to],
    [FILE_PATH_101_3_from, FILE_PATH_101_3_to]
  ]

  for i in range(len(fileNames)):
    fileName = fileNames[i]
    handle(fileName[0], fileName[1], i)